/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "HoughAngleMatcher.h"

////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/// Generic Quicksort for vectors
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
using namespace std;
template <class T> unsigned int pivot (vector<T> & v, unsigned int start, unsigned int stop, unsigned int position)
	// partition vector into two groups
	// values smaller than or equal to pivot
	// values larger than pivot
	// return location of pivot element
{
		// swap pivot into starting position
		swap (v[start], v[position]);

		// partition values
		unsigned int low = start + 1;
		unsigned int high = stop;
		while (low < high)
				if (v[low] < v[start])
						low++;
		else if (v[--high] < v[start])
				swap (v[low], v[high]);

		// then swap pivot back into place
		swap (v[start], v[--low]);
		return low;
}

template <class T> void quickSort(vector<T> & v, unsigned int low, unsigned int high){
	// no need to sort a vector of zero or one elements
		if (low >= high) return;

	// select the pivot value
		unsigned int pivotIndex = (low + high) / 2;

	// partition the vector
		pivotIndex = pivot (v, low, high, pivotIndex);

	// sort the two sub vectors
		if (low < pivotIndex)
				quickSort(v, low, pivotIndex);
		if (pivotIndex < high)
				quickSort(v, pivotIndex + 1, high);
}
template <class T> void quickSort(vector<T> & v){
		unsigned int numberElements = v.size ();
		if (numberElements > 1) 
				quickSort(v, 0, numberElements - 1);
}

/**
 * Sorts the matches
 **/
void CHoughAngleMatcher::sort(std::vector<TAngleMatch> &matches){
		quickSort(matches);
	///GLUDGE 
		if(matches.size()>1){
				if(matches[matches.size()-1].corrVal < matches[matches.size()-2].corrVal){
						vector<TAngleMatch> tmp = matches; ///< stupid copy
						matches.clear(); 									///< clear this
						unsigned int i=0;
						while(tmp[i].corrVal < tmp[tmp.size()-1].corrVal){
								matches.push_back(tmp[i]);
								i++;
						}
						matches.push_back(tmp[tmp.size()-1]);
						for(unsigned int j=i;j<tmp.size()-1;j++) matches.push_back(tmp[j]);
				}
		} 
}


/**
 * Constructor
 * @param numberofrangevalues Fixed Number, which is used in discretation
 * @param numberofanglevalues Fixed Number, which is used in discretation 
 */
CHoughAngleMatcher::CHoughAngleMatcher(int numberofrangevalues, int numberofanglevalues)
{
		numRangeValues = numberofrangevalues;
		numThetaValues = numberofanglevalues;
		float ad = M_PI/(numThetaValues-1);
		
		sinlut=(float *) malloc(sizeof(float)*numThetaValues);
		coslut=(float *) malloc(sizeof(float)*numThetaValues);
		houghTf1=(float *) malloc(sizeof(float)*numThetaValues*numRangeValues);
		houghTf2=(float *) malloc(sizeof(float)*numThetaValues*numRangeValues);
		houghSpec1=(float *) malloc(sizeof(float)*numThetaValues);
		houghSpec2=(float *) malloc(sizeof(float)*numThetaValues);
		corr = (float *) malloc(sizeof(float)*numThetaValues);
		
		if(!sinlut || !coslut || !houghTf1 || !houghTf2 || !houghSpec1 || !houghSpec2 || !corr){
				fprintf(stderr,"Malloc error - Abort!\n ");
				exit(1);
		}
		
		///Cache the discrete angle values
		for(int j=0;j<numThetaValues;j++){
				coslut[j]=cos(j*ad);
				sinlut[j]=sin(j*ad);
		}
}

CHoughAngleMatcher::~CHoughAngleMatcher(){
		if(sinlut)free(sinlut);
		if(coslut)free(coslut);
		if(houghTf1)free(houghTf1);
		if(houghTf2)free(houghTf2);
		if(houghSpec1)free(houghSpec1);
		if(houghSpec2)free(houghSpec2);
		if(corr) free(corr);
}


/**
 * Finds a set of candidate angles between two given datasets
 * The method assumes that the data points are taken approximately from
 * the same environment, but from different angles. The mathing
 * is based on correlating the HoughSpectrum of both sets.
 * The result is returned as a (sorted) vector of angle, correlation pairs
 * The larger the correlation the better.
 * @param &ref reference dataset
 * @param &meas measurement datasets
 * @param MaxR Maximum range (from origin) that the data point can lie in
 * @param &matches the result
 */
void CHoughAngleMatcher::match(CEnvironmentMeasurement2D &ref, 
									 CEnvironmentMeasurement2D &meas, 
									 float MaxR,
									 std::vector<TAngleMatch> &matches)
{
		std::vector<int> ind;
		///Compute transformations
		houghtransform(houghTf1,ref, MaxR);
		houghtransform(houghTf2,meas, MaxR);
		
		///Compute Spectrums
		computeHoughSpectrum(houghTf1, houghSpec1);
		computeHoughSpectrum(houghTf2, houghSpec2);
		
		///Correlate Spectrums
		computeCorrelation();
		///Find maximums 
		findMaximumCorrelations(ind);
		
		///Store the values to the match structure (and duplicate the angle to +- result)
		for(unsigned int i=0;i<ind.size();i++){
				float a = ((float)ind[i])/(float)numThetaValues * M_PI;
				matches.push_back(TAngleMatch(a,corr[ind[i]]/1000.0));
				matches.push_back(TAngleMatch(a-M_PI,corr[ind[i]]/1000.0));
		}
		
		//sort(matches);
}

/**
 * Make a Hough Transform 
 * @param *hth The hough transform
 * @param &meas EnvironmentMeasurement2D object containing the (x,y) points
 * @param MaxR   Maximum range (from origin to furthest point)
 **/
void CHoughAngleMatcher::houghtransform(float *hth,CEnvironmentMeasurement2D &meas, float MaxR){
		int k;
		float r;
		for(unsigned int i=0;i<meas.size();i++){
				for(int j=0;j<numThetaValues;j++){
						///lasketaan r suoran yhtälöstä 
						///-maxx<r<sqrt(max^2+maxy^2),maxx=576,maxR=sqrt(max^2+maxy^2)
						r=meas.points[i].x*coslut[j]+meas.points[i].y*sinlut[j]+MaxR;
						///muunnetaan r-arvo indeksiksi
						k=(int)((numRangeValues/(MaxR+MaxR))*r);
						///lisätään solun osumalaskuria yhdellä.
						hth[k+j*numRangeValues]++;
				}
		}
}

/**
 * Compute Hough Spectrum
 * @param *hth pointer to Hough Transformation
 * @param *spectrum pointer to spectrum (output)
 */
void CHoughAngleMatcher::computeHoughSpectrum(float *hth, float *spectrum){
		/* hough spektrien lasku */
		for(int i=0;i<numThetaValues;i++){
				spectrum[i]=hth[i*numRangeValues]*hth[i*numRangeValues];
				for(int j=1;j<numRangeValues;j++){
						spectrum[i]+=hth[j+i*numRangeValues]*hth[j+i*numRangeValues];
				}
		}
}

/**
 * Calculates the correlation between Hough Spectrums
 * Uses the class variables @houghSpec1 and @houghSpec2 and
 * stores the result into class variable @corr
 */
void CHoughAngleMatcher::computeCorrelation(){
		for(int i=0;i<numThetaValues;i++){
				corr[i]=houghSpec1[0]*houghSpec2[i];
				for(int j=1;j<numThetaValues-i;j++){
						corr[i]+=houghSpec1[j]*houghSpec2[i+j];
				}
	
				for(int j=numThetaValues-i;j<numThetaValues;j++){
						corr[i]+=houghSpec1[j]*houghSpec2[j-numThetaValues+i];
				}
		}
}
			
/**
 * Finds all local maximas from the correlation function
 * Uses the class variable  @corr and assumes it is already calculated
 * @param &out vector of indices that corresponds to local maximas
 */
void CHoughAngleMatcher::findMaximumCorrelations(std::vector<int> &out){
		out.clear(); ///< make sure its empty
		
		if((corr[0]>corr[1])&&(corr[0]>corr[numThetaValues-1]))out.push_back(0);
	
		for(int i=1;i<numThetaValues-1;i++){
				if((corr[i]>corr[i+1])&&(corr[i]>corr[i-1])) out.push_back(i);
		}
	
		if((corr[numThetaValues-1]>corr[0])&&(corr[numThetaValues-1]>corr[numThetaValues-2])){
				out.push_back(numThetaValues-1);
		}
}
